/*
 * Copyright 2009 Gordon Turner
 * Copyright 2008 Garth Shoemaker
 * 
 * This file is part of openwiimote.
 *
 * openwiimote is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * openwiimote is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with Wiimote Simple.  If not, see <http://www.gnu.org/licenses/>.
 */

package org.openwiimote;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.LinkedList;
import java.util.ListIterator;

import javax.bluetooth.L2CAPConnection;
import javax.microedition.io.Connector;

import org.apache.log4j.Logger;
import org.openwiimote.event.WiiAccelEvent;
import org.openwiimote.event.WiiButtonEvent;
import org.openwiimote.event.WiiEvent;
import org.openwiimote.event.WiiIrEvent;
import org.openwiimote.event.WiimoteListener;

/*
 * Represents a single wiimote that has been connected. This will normally
 * be provided for by a WiimoteDiscoverer. Once discovered, a WiimoteListener 
 * can be registered to receive Wiimote events.
 */
public class Wiimote
{
  Logger logger = Logger.getLogger( WiimoteDiscoverer.class );

  String address;

  protected LinkedList listeners;

  protected L2CAPConnection sendCon;

  protected L2CAPConnection receiveCon;

  boolean connectionOpen;

  protected int light;

  protected Wiimote wiimote;

  protected WiiButtonEvent lastButtonEvent;

  final protected static byte COMMAND_LIGHT = 0x11;

  final protected static byte COMMAND_IR = 0x13;

  final protected static byte COMMAND_IR_2 = 0x1a;

  final protected static byte COMMAND_REGISTER = 0x16;

  final protected static byte COMMAND_REPORTING = 0x12;

  final protected static byte COMMAND_READ_CALIBRATION = 0x17;

  double[] calibrationZero, calibrationOne;

  CommandListener commandListener;

  static Object gate = new Object();

  boolean inited = false;

  /**
   * Creates a Wiimote instance given an address. Forms connections to the 
   * wiimote, and readies sending and receiving of data. You likely won't call 
   * this yourself. Instead, your Wiimote instances will be created by 
   * WiimoteDiscoverer.
   */
  public Wiimote( String a ) throws Exception
  {
    logger.warn( "BEGIN" );
    logger.debug( Thread.currentThread().getName() );

    connectionOpen = false;
    calibrationZero = new double[ 3 ];
    calibrationOne = new double[ 3 ];

    listeners = new LinkedList();
    light = -1;
    wiimote = this;
    address = "btl2cap://" + a;

    try
    {
      /* 
       * The L2CAPConnections can take a long time to open, catch the exception
       * if they can't be opened.
       */
      sendCon = (L2CAPConnection) Connector.open( address + ":11",
          Connector.WRITE, true );

      logger.debug( "Opened sendCon: " + address + ":11" );

      receiveCon = (L2CAPConnection) Connector.open( address + ":13",
          Connector.READ, true );

      logger.debug( "Opened receiveCon: " + address + ":13" );

      connectionOpen = true;
    }
    // Attempt to clean up the connections.
    catch( Exception e )
    {
      if( receiveCon != null )
      {
        receiveCon.close();
      }

      if( sendCon != null )
      {
        sendCon.close();
      }

      logger.error( "Could not open L2CAPConnection connections." );
      throw e;
    }

    logger.debug( "Creating CommandListener and starting the thread." );
    commandListener = new CommandListener();
    commandListener.start();
    readCalibration();

    logger.warn( "END" );
  }

  /**
   * Attaches a WiimoteListener to this wiimote. The wiimote will send events 
   * to the given listener whenever something happens (e.g. button is pressed, 
   * accelerometer event occurs).
   */
  public void addListener( WiimoteListener l )
  {
    if( !listeners.contains( l ) )
    {
      listeners.add( l );
    }
  }

  /**
   * Detaches the given WiimoteListener.
   */
  public void removeListener( WiimoteListener l )
  {
    listeners.remove( l );
  }

  /**
   * Dispatches wiimote events to all the WiimoteListeners.
   */
  protected void dispatchEvent( WiiEvent wiiEvent )
  {
    for( ListIterator it = listeners.listIterator(); it.hasNext(); )
    {
      WiimoteListener listener = (WiimoteListener) it.next();

      if( wiiEvent instanceof WiiButtonEvent )
      {
        listener.wiiButtonChange( (WiiButtonEvent) wiiEvent );
      }

      if( wiiEvent instanceof WiiIrEvent )
      {
        listener.wiiIRInput( (WiiIrEvent) wiiEvent );
      }

      if( wiiEvent instanceof WiiAccelEvent )
      {
        listener.wiiAccelInput( (WiiAccelEvent) wiiEvent );
      }
    }
  }

  /**
   * 
   * The initialization routine is taken from:
   * 
   * http://wiibrew.org/Wiimote.html#Initialization
   * 
   */
  public void enableIREvents()
  {
    // 0x33: Core Buttons and Accelerometer with 12 IR bytes 
    sendCommand( COMMAND_REPORTING, new byte[] { 0x04, 0x33 } );

    try
    {
      // Send 0x04 to Output Report 0x13.
      sendCommand( COMMAND_IR, new byte[] { 0x04 } );
      Thread.sleep( 100 );

      // Send 0x04 to Output Report 0x1a.
      sendCommand( COMMAND_IR_2, new byte[] { 0x04 } );
      Thread.sleep( 100 );

      // Write 0x08 to register 0xb00030.
      writeToRegister( 0xb00030, new byte[] { 0x08 } );
      Thread.sleep( 100 );

      // Write Sensitivity Block 1 to registers at 0xb00000.
      writeToRegister( 0xb00000, new byte[] { 0x00, 0x00, 0x00, 0x00, 0x00,
          0x00, (byte) 0x90, 0x00, (byte) 0xC0 } );
      Thread.sleep( 100 );

      // Write Sensitivity Block 2 to registers at 0xb0001a.
      writeToRegister( 0xb0001a, new byte[] { 0x40, 0x00 } );
      Thread.sleep( 100 );

      // Write Mode Number to register 0xb00033, 1=basic 3=extended 5=full.
      // http://wiibrew.org/Wiimote.html#Data_Formats
      writeToRegister( 0xb00033, new byte[] { (byte) 3 } );
      Thread.sleep( 100 );

      // Write 0x08 to register 0xb00030 (again).
      writeToRegister( 0xb00030, new byte[] { 0x08 } );
      Thread.sleep( 100 );
    }
    catch( Exception e )
    {
      logger.error( "enableIREvents exception: ", e );
    }
  }

  /**
   * TODO: Test this, not known if this works.
   */
  public void disableIREvents()
  {
    System.out.println( "WARNING: disableIREvents() IS UNTESTED!" );

    try
    {
      // Send 0x00 to Output Report 0x13 to turn off.
      sendCommand( COMMAND_IR, new byte[] { 0x00 } );
      Thread.sleep( 100 );

      // Send 0x00 to Output Report 0x1a ti turn off.
      sendCommand( COMMAND_IR_2, new byte[] { 0x00 } );
      Thread.sleep( 100 );
    }
    catch( Exception e )
    {
      logger.error( "disableIREvents exception: ", e );
    }
  }

  /*
   * 0x32: Core Buttons with 8 Extension bytes 
   */
  public void enableCoreButtonsWith8Extensions()
  {
    logger.info( "Setting to 0x32: Core Buttons with 19 Extension bytes." );
    sendCommand( COMMAND_REPORTING, new byte[] { 0x04, 0x32 } );
  }

  /*
   * 0x34: Core Buttons with 19 Extension bytes.
   */
  public void enableCoreButtonsWith19Extensions()
  {
    logger.info( "Setting to 0x34: Core Buttons with 19 Extension bytes." );
    sendCommand( COMMAND_REPORTING, new byte[] { 0x04, 0x34 } );
  }

  /*
   * 0x30: Core Buttons.
   */
  public void enableCoreButtons()
  {
    logger.info( "Setting to 0x30: Core Buttons." );
    sendCommand( COMMAND_REPORTING, new byte[] { 0x04, 0x34 } );
  }

  /**
   * Turns on the given light on the wiimote. 
   * 
   * Lights are indexed from 0.
   */
  public void setLight( int i )
  {
    if( i < 0 || i > 3 )
    {
      return;
    }

    light = i;
    sendCommand( COMMAND_LIGHT, new byte[] { (byte) Math.pow( 2, 4 + i ) } );
  }

  /**
   * Returns which light is turned on on this wiimote
   */
  public int getLight()
  {
    return light;
  }

  /**
   * Causes the wiimote to vibrate for the given number of milliseconds.
   */
  public void vibrate( final int millis )
  {
    Thread thread = new Thread( new Runnable() {
      public void run()
      {
        sendCommand( COMMAND_LIGHT, new byte[] { (byte) ( 0x01 | (byte) ( Math
            .pow( 2, 4 + light ) ) ) } );
        try
        {
          Thread.sleep( millis );
        }
        catch( Exception e )
        {
          logger.error( "vibrate exception: ", e );
        }
        sendCommand( COMMAND_LIGHT, new byte[] { (byte) ( Math.pow( 2,
            4 + light ) ) } );
      }
    } );
    thread.start();
  }

  /**
   * Causes the given data to be written to the given register address on the 
   * wiimote.
   */
  private void writeToRegister( int address, byte[] data )
  {
    byte[] message = new byte[ 21 ];

    message[ 0 ] = 0x04;

    message[ 1 ] = (byte) ( address >> 16 & 0xff );
    message[ 2 ] = (byte) ( address >> 8 & 0xff );
    message[ 3 ] = (byte) ( address & 0xff );

    message[ 4 ] = (byte) data.length;
    for( int i = 0; i < data.length; i++ )
    {
      message[ 5 + i ] = data[ i ];
    }
    sendCommand( COMMAND_REGISTER, message );
  }

  /**
   * Sends message to the wiimote asking for accelerometer calibration data.
   */
  synchronized private void readCalibration()
  {
    try
    {
      byte[] message = new byte[] { 0x00, 0x00, 0x00, 0x16, 0x00, 0x08 };
      sendCommand( COMMAND_READ_CALIBRATION, message );
    }
    catch( Exception e )
    {
      logger.error( "readCalibration exception: ", e );
    }
  }

  /**
   * Sends a generic command to the wiimote.
   */
  synchronized private void sendCommand( byte command, byte[] payload )
  {
    try
    {
      byte[] message = new byte[ 2 + payload.length ];
      message[ 0 ] = 82;
      message[ 1 ] = command;
      System.arraycopy( payload, 0, message, 2, payload.length );

      sendCon.send( message );
    }
    catch( IOException e )
    {
      //logger.info( "error sending data ", e );
    }
  }

  /**
   * Causes the wiimote connections to close.
   */
  protected void finalize() throws Throwable
  {
    cleanup();
  }

  /**
   * Close any open wiimote connections.
   */
  public void cleanup()
  {
    logger.debug( "BEGIN" );

    synchronized( receiveCon )
    {
      connectionOpen = false;
      try
      {
        if( sendCon != null )
        {
          sendCon.close();
        }

        if( receiveCon != null )
        {
          receiveCon.close();
        }

        if( commandListener != null )
        {
          commandListener.interrupt();
        }

      }
      catch( Exception e )
      {
        logger.error( "cleanup exception: ", e );
      }
    }

    commandListener.setCommandListenerRun( new Boolean( false ) );

    logger.debug( "END" );
  }

  /**
   * This loops infinitely, constantly listening for data from the wiimote.
   * 
   * When data is received it responds accordingly.
   */
  protected class CommandListener extends Thread
  {
    private volatile Boolean commandListenerRun = new Boolean( true );

    public synchronized Boolean getCommandListenerRun()
    {
      return commandListenerRun;
    }

    public synchronized void setCommandListenerRun( Boolean commandListenerRun )
    {
      this.commandListenerRun = commandListenerRun;
    }

    public void run()
    {
      logger.debug( "BEGIN" );

      while( commandListenerRun.booleanValue() )
      {
        byte[] bytes = new byte[ 32 ];
        if( connectionOpen == true )
        {
          try
          {
            logger.debug( "Blocking until data ready...." );
            receiveCon.receive( bytes );
          }
          catch( IOException e )
          {
            // Do nothing.
          }
          catch( Exception e )
          {
            logger.error( "wiimote " + receiveCon + " " + light
                + " exception: receive(bytes): ", e );
          }

          /*
           * This where the Input Reports are translated into an event.
           */
          switch( 0xFF & bytes[ 1 ] )
          {
            case 0x21:
              // Read Memory Data 
              // http://wiibrew.org/wiki/Wiimote#0x21:_Read_Memory_Data
              parseCalibrationResponse( ByteBuffer.allocate( 8 ).put( bytes, 7,
                  8 ) );
              break;
            case 0x30:
              // Core Buttons 
              // http://wiibrew.org/wiki/Wiimote#0x30:_Core_Buttons
              createButtonEvent( bytes );
              break;
            case 0x33:
              // Core Buttons and Accelerometer with 12 IR bytes 
              // http://wiibrew.org/wiki/Wiimote#0x33:_Core_Buttons_and_Accelerometer_with_12_IR_bytes
              createButtonEvent( bytes );
              createAccelEvent( ByteBuffer.allocate( 5 ).put( bytes, 2, 5 ) );
              createIREvent( ByteBuffer.allocate( 12 ).put( bytes, 7, 12 ) );
              break;
          }
        }
        else
        {
          try
          {
            Thread.sleep( 1000 );
          }
          catch( Exception e )
          {
            // Do nothing.
          }
        }
      }

      logger.debug( "END" );
    }

    /**
     * When a response to an accelerometer calibration request is received this
     * parses it.
     */
    protected void parseCalibrationResponse( ByteBuffer b )
    {
      calibrationZero[ 0 ] = ( ( b.get( 0 ) & 0xFF ) << 2 ) + ( b.get( 3 ) & 3 );
      calibrationZero[ 1 ] = ( ( b.get( 1 ) & 0xFF ) << 2 )
          + ( ( b.get( 3 ) & 0xC ) >> 2 );
      calibrationZero[ 2 ] = ( ( b.get( 2 ) & 0xFF ) << 2 )
          + ( ( b.get( 3 ) & 0x30 ) >> 4 );

      calibrationOne[ 0 ] = ( ( b.get( 4 ) & 0xFF ) << 2 ) + ( b.get( 7 ) & 3 );
      calibrationOne[ 1 ] = ( ( b.get( 5 ) & 0xFF ) << 2 )
          + ( ( b.get( 7 ) & 0xC ) >> 2 );
      calibrationOne[ 2 ] = ( ( b.get( 6 ) & 0xFF ) << 2 )
          + ( ( b.get( 7 ) & 0x30 ) >> 4 );
    }

    /**
     * Creates a WiiButtonEvent from raw data.
     */
    protected void createButtonEvent( byte[] bytes )
    {
      /*
       * The first two bytes for almost all reports the core button presses.
       * 
       * When a button is pressed the report and corresponding bytes are sent,
       * when the button is released, a report with '0' '0' bytes are sent.
       */
      WiiButtonEvent event = new WiiButtonEvent( wiimote, bytes[ 2 ], bytes[ 3 ] );

      /*
       *  This prevents repeating events, if the event is different then the
       *  last one, don't dispatch the event.
       */
      if( !event.equals( lastButtonEvent ) )
      {
        lastButtonEvent = event;
        wiimote.dispatchEvent( event );
      }
    }

    /**
     * Creates a WiiAccelEvent from raw data
     */
    protected void createAccelEvent( ByteBuffer b )
    {
      int x = ( ( b.get( 2 ) & 0xff ) << 2 ) + ( ( b.get( 0 ) & 0x60 ) >> 5 );
      int y = ( ( b.get( 3 ) & 0xff ) << 2 ) + ( ( b.get( 1 ) & 0x60 ) >> 5 );
      int z = ( ( b.get( 4 ) & 0xff ) << 2 ) + ( ( b.get( 1 ) & 0x80 ) >> 6 );

      double xaccel = ( (double) x - calibrationZero[ 0 ] )
          / ( calibrationOne[ 0 ] - calibrationZero[ 0 ] );
      double yaccel = ( (double) y - calibrationZero[ 1 ] )
          / ( calibrationOne[ 1 ] - calibrationZero[ 1 ] );
      double zaccel = ( (double) z - calibrationZero[ 2 ] )
          / ( calibrationOne[ 2 ] - calibrationZero[ 2 ] );

      WiiAccelEvent event = new WiiAccelEvent( wiimote, xaccel, yaccel, zaccel );
      wiimote.dispatchEvent( event );
    }

    /**
     * Creates a WiiIREvent from raw data
     */
    protected void createIREvent( ByteBuffer b )
    {
      for( int i = 0; i < b.limit(); i += 3 )
      {
        int x = ( b.get( i ) & 0xFF ) + ( ( b.get( i + 2 ) & 0x30 ) << 4 );
        int y = ( b.get( i + 1 ) & 0xFF ) + ( ( b.get( i + 2 ) & 0xc0 ) << 2 );
        int size = ( b.get( i + 2 ) & 0xF );
        if( x != 1023 && y != 1023 )
        {
          WiiIrEvent event = new WiiIrEvent( wiimote, i, x, y, size );
          wiimote.dispatchEvent( event );
        }
      }
    }

    /**
     * Convert a byte[] array to readable string format. This makes the "hex"
     * readable!
     *
     * @return result String buffer in String format 
     * @param byteArray byte[] buffer to convert to string format
     */
    protected String byteArrayToHexString( byte byteArray[] )
    {
      byte ch = 0x00;
      int i = 0;

      if( byteArray == null || byteArray.length <= 0 )
      {
        return null;
      }

      String pseudo[] = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9",
          "A", "B", "C", "D", "E", "F" };

      StringBuffer out = new StringBuffer( byteArray.length * 2 );

      while( i < byteArray.length )
      {
        // Strip off  high nibble
        ch = (byte) ( byteArray[ i ] & 0xF0 );

        // shift the bits down
        ch = (byte) ( ch >>> 4 );

        // must do this is high order bit is on!
        ch = (byte) ( ch & 0x0F );

        // convert the nibble to a String Character
        out.append( pseudo[ (int) ch ] );

        // Strip off low nibble
        ch = (byte) ( byteArray[ i ] & 0x0F );

        // convert the nibble to a String Character
        out.append( pseudo[ (int) ch ] );

        i++;
      }

      String rslt = new String( out );

      return rslt;

    }

    /**
     * Convert a byte[] array to readable string format.
     * 
     * Original code:
     * 
     * http://www.devx.com/tips/Tip/13540
     *
     * @return result String
     * @param byte to convert to string
     */
    protected String byteToHexString( byte byteInput )
    {
      byte ch = 0x00;

      String pseudo[] = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9",
          "A", "B", "C", "D", "E", "F" };

      StringBuffer out = new StringBuffer( 2 );
      // Strip off  high nibble
      ch = (byte) ( byteInput & 0xF0 );

      // shift the bits down
      ch = (byte) ( ch >>> 4 );

      // must do this is high order bit is on!
      ch = (byte) ( ch & 0x0F );

      // convert the nibble to a String Character
      out.append( pseudo[ (int) ch ] );

      // Strip off low nibble
      ch = (byte) ( byteInput & 0x0F );

      // convert the nibble to a String Character
      out.append( pseudo[ (int) ch ] );

      return new String( out );
    }
  }
}
